/*
 * Controller.cpp
 *
 *  Created on: Apr 24, 2011
 *      Author: tflanzer
 */

#include <termios.h>
#include "Controller.h"
#include "mavlink.h"

#define PI 3.141592653589793

Controller::Controller(int fa, int fg, FileWriter *fwi, Guidance *gnci) {

	faup = fa;
	fgnd = fg;
	fw = fwi;

	philim = ctrlGns.cmap["philim"];
	Vtarget = ctrlGns.cmap["Vtarget"];
	dDes = ctrlGns.cmap["dDes"];

	ail_PWM = 127; //pv.ail_trim;
	ele_PWM = 127; //pv.rud_trim;
	rud_PWM = 127; //pv.rud_trim;
	thr_PWM = 0;

	ail_lim = 10.0*M_PI/180.0; //16.5*M_PI/180.0;
	ele_lim = 10.0*M_PI/180.0; //15*M_PI/180.0;
	rud_lim = 10.0*M_PI/180.0; //25*M_PI/180.0;

	aileron  = new Servo(-301.45,-50.215,-254.73,133.01,ail_lim);
	elevator = new Servo(272.2,-62.278,208.11,149.19,ele_lim);
	rudder   = new Servo(186.61,-9.6371,112.4,135.54,rud_lim);

	// Initialize constants
	U0   = 10;
	taup = 5;
	philim = 30*M_PI/180.0;

	// Initialize states
	xe[0] = 0;  xe[1] = 0;  xe[2] = 0;
	xeL[0] = 0; xeL[1] = 0; xeL[2] = 0;
	phiL = 0;   re[0] = 0;  re[1] = 0;
	VaL = 8;   hL = 50;
	numwps = 4;

	// Initialize error states to zero
	phiErr = 0; rErr = 0; dErr = 0; vErr = 0;

	// Initialize desired bank and heading angles
	phides = 0.0;
	psiDes = 0.0;

	flag = true; // Autopilot detection flag

	// Initialize control deflections to zero
	cv.ailDefRad = 0.0;
	cv.eleDefRad = 0.0;
	cv.rudDefRad = 0.0;
	cv.thrPercent = 0.0;

	// Initialize guidance
	gnc = gnci;

	// Used for system ID counting
	sysctrLon = 0; sysctrLat = 0;
	sysflagLon = false; sysflagLat = false;
	vertManFlg = false; ctr = 0;
}

Controller::~Controller() {
}

void Controller::fiftyHzUpdate(Timer &timer) {

	// For debugging run guidance loop even if ap not engaged
	//phides = gnc->getBankAngleCmd();

	Vtarget = ctrlGns.cmap["Vtarget"]; //gnc->Vtarget;
	dDes = ctrlGns.cmap["dDes"]; //gnc->dDes;

	if ((flag) && apdat.autoFlag) {
		flag = false;
		psiDes = sv.psi;
	} else if (!flag && !apdat.autoFlag) {
		flag = true;
	}

	// Only run control loop if autopilot is engaged
	if (apdat.autoFlag) {

		// Hold heading when autopilot is turned on
		double deltaPsi = psiDes - sv.psi;
		while (deltaPsi > M_PI) {
		    deltaPsi = deltaPsi - 2*M_PI;
		}
		while (deltaPsi < -M_PI) {
		    deltaPsi = deltaPsi + 2*M_PI;
		}
		phides = U0/(taup*9.81)*deltaPsi;
		phides = fmax(fmin(phides,philim),-philim);

		// Track Vtarget and dDes
		LQRPI_V_alt();

		// Run pbeta lateral loop
		pbeta(phides);

		// System identification inputs
		if (sysflagLon) sysIDLong();
		if (sysflagLat) sysIDLat();
		if (vertManFlg) {
			pbeta(0.0);
			Vtarget = 10.0 + 5.0*cos((double)ctr/251.0*2*M_PI);
			dDes = -85.0 + 5.0*cos((double)ctr/251.0*2*M_PI);
			cout << "Vt = " << Vtarget << " dDes = " << dDes << endl;
			LQRPI_V_alt();
			ctr++;
		} else {
			ctr = 0.0;
		}

		// Send servo commands to the autopilot
		sendServoCommands();

		// Get target altitude and airspeed
//		dDes = ctrlGns.cmap["dDes"];
//		Vtarget = ctrlGns.cmap["Vtarget"];

		// Update error states
		phiErr = phiErr + (sv.phi - phides)*0.02;
		rErr   = rErr   + sv.r*0.02;
		dErr   = dErr   + (sv.Dp - dDes)*0.02;
		vErr   = vErr   + (computeAirspeed()-Vtarget)*0.02;

		phiErr = fmax(fmin(phiErr,1),-1);
		rErr   = fmax(fmin(rErr,1),-1);
		dErr   = fmax(fmin(dErr,10),-10);
		vErr   = fmax(fmin(vErr,5),-5);

		// Pack previous states
		xeL[0] = xe[0];
		xeL[1] = xe[1];
		xeL[2] = xe[2];
		phiL   = sv.phi;
		rL     = sv.r;
		re[0]  = re[1];
		Va_err_i[0] = Va_err_i[1];

		// Update control deflection values
		updateControlFile();

		fw->updateValue("phides",phides);

		// Record target velocity, altitude
		fw->updateValue("dDes",dDes);
		fw->updateValue("VDes",Vtarget);
	}
}

void Controller::sendServoCommands() {

	uint8_t buf[10];
	uint8_t *ptr;
	ptr = buf;

	*ptr = 83;
	*(ptr+1) = (uint8_t)ail_PWM;
	*(ptr+2) = (uint8_t)ele_PWM;
	*(ptr+3) = (uint8_t)thr_PWM;
	*(ptr+4) = (uint8_t)rud_PWM;

	uint8_t chksum = 0;
	for (int i = 1; i<5; i++) {
		chksum ^= buf[i];
	}

	*(ptr+5) = chksum;

	int n = write(faup,buf,6);
	if (n < 0) {
	} else {
		nums++;
	}
}

void Controller::GNDdataReceived(Communication &com) {

	if (com.gndmsgtype == MAVLINK_MSG_ID_SET_MODE) {

		cout << "Commanded mode = " << +com.MAVLINK_MODE << endl;

		if ((int)com.MAVLINK_MODE==1) {

		} else if ((int)com.MAVLINK_MODE==2) {
			if (!sysflagLon) {
				cout << "Longitudinal system identification activated" << endl;
				sysflagLon = true;
			} else {
				cout << "Longitudinal system identification deactivated" << endl;
				sysflagLon = false;
			}

		} else if ((int)com.MAVLINK_MODE==3) {
			if (!sysflagLat) {
				cout << "Lateral system identification activated" << endl;
				sysflagLat = true;
			} else {
				cout << "Lateral system identification deactivated" << endl;
				sysflagLat = false;
			}

		} else if ((int)com.MAVLINK_MODE==4) {
			if (!vertManFlg) {
				cout << "Vertical maneuver mode activated" << endl;
				vertManFlg = true;
			} else {
				cout << "Vertical maneuver mode deactivated" << endl;
				vertManFlg = false;
			}

		} else if ((int)com.MAVLINK_MODE==5) {

		} else if ((int)com.MAVLINK_MODE==6) {
			// Set current location as center
			for (int i = 0; i < NB+1; i++) {
				gnc->wp[0][i] = gnc->wp0[0][i] + sv.Np;
				gnc->wp[1][i] = gnc->wp0[1][i] + sv.Ep;
			}
			cout << "Waypoint center location updated to N = " << sv.Np << " and Ep = "
					<< sv.Ep << endl;
		}
	}
}

double Controller::wpCommand() {
/*
	double Xe, Ye, psi, Xwp, Ywp, psi_c, dWP, x0, y0, x1[3], x2[3], nx2mx1,
		d, dL, d_dot, x2mx1[3], x1mxe[3], x1mxeL[3], c1[3], c2[3], n[2], r[2],
		V_L1, as, dnr, deltaPsi, derr, phi_d;

	// Current heading angle
	psi = atan2(sv.VEi,sv.VNi);

	// Current position
	Xe = sv.Np;
	Ye = sv.Ep;

	// Desired waypoint
	Xwp = pv.waypointNorth[pv.wpidx];
	Ywp = pv.waypointEast[pv.wpidx];

	// Compute desired heading command, psi_c
	psi_c = atan2(Ywp-Ye,Xwp-Xe);

	// Difference between desired heading and current heading
	deltaPsi = psi_c - psi;

	while (deltaPsi > M_PI) {
	    deltaPsi = deltaPsi - 2*M_PI;
	}
	while (deltaPsi < -M_PI) {
	    deltaPsi = deltaPsi + 2*M_PI;
	}

	// Check to see if waypoint has been reached (within 10m)
	dWP = sqrt((Xe-Xwp)*(Xe-Xwp)+(Ye-Ywp)*(Ye-Ywp));
	if (dWP < 10) {
	    // Waypoint reached, send message to ground station
		mavlink_message_t msg;
		uint8_t buf[MAVLINK_MAX_PACKET_LEN];
		mavlink_msg_waypoint_reached_pack(100, 1, &msg, (uint16_t) pv.wpidx);
		uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
		int n = write(fgnd,buf,len);
		if (n < 0) {
			cout << "Waypoint reached command not sent" << endl;
		}

	    if (pv.wpidx == numwps-1) {
	    	pv.wpidx = 0;
	    } else {
	    	pv.wpidx++;
	    }
	}

	// Else add correction so that aircraft follows a straight path from the
	// previous waypoint to the commanded one.
	if (dWP > 25) {
	    // Previous waypoint
		if (pv.wpidx == 0) {
			x0 = pv.waypointNorth[numwps-1];
			y0 = pv.waypointNorth[numwps-1];
		} else {
			x0 = pv.waypointNorth[pv.wpidx-1];
			y0 = pv.waypointNorth[pv.wpidx-1];
		}

		x1[0] = x0;  x1[1] = y0;  x1[2] = 0;
		x2[0] = Xwp; x2[1] = Ywp; x2[2] = 0;
		xe[0] = Xe;  xe[1] = Ye;  xe[2] = 0;

		nx2mx1 = sqrt((x2[0]-x1[0])*(x2[0]-x1[0])+(x2[1]-x1[1])*(x2[1]-x1[1])+
				(x2[2]-x1[2])*(x2[2]-x1[2]));
	    if (nx2mx1 != 0) {
	    	x2mx1[0] = x2[0]-x1[0];
	    	x2mx1[1] = x2[1]-x1[1];
	    	x2mx1[2] = x2[2]-x1[2];
	    	x1mxe[0] = x1[0]-xe[0]; x1mxeL[0] = x1[0]-xeL[0];
	    	x1mxe[1] = x1[1]-xe[1]; x1mxeL[1] = x1[1]-xeL[1];
	    	x1mxe[2] = x1[2]-xe[2]; x1mxeL[2] = x1[2]-xeL[2];
	    	// c1 = (x2-x1) x (x1-xe)
	    	cross(x2mx1,x1mxe,c1);
	    	// c2 = (x2-x1) x (x1-xeL)
	    	cross(x2mx1,x1mxeL,c2);
	    	d = sqrt(c1[0]*c1[0]+c1[1]*c1[1]+c1[2]*c1[2])/nx2mx1;
	    	dL = sqrt(c2[0]*c2[0]+c2[1]*c2[1]+c2[2]*c2[2])/nx2mx1;
	        d_dot = (d-dL)/(0.02);
	    } else {
	        d_dot = 0;
	        d = 0;
	    }
	    n[0] = Ywp-y0; n[1] = x0-Xwp;
	    r[0] = x0-Xe;  r[1] = y0-Ye;
	    V_L1 = 1.0/10.0;
	    as = 2*V_L1*(d_dot + V_L1*d);
	    dnr = n[0]*r[0]+n[1]*r[1];
	    if (dnr >= 0) {
	    	derr = -atan(as/9.81);
	    } else {
	    	derr = atan(as/9.81);
	    }
	    if (derr > M_PI/6.0) {
	        derr = M_PI/6.0;
	    } else if (derr < -M_PI/6.0) {
	        derr = -M_PI/6.0;
	    }
	} else {
	    derr = 0;
	}
	phi_d = U0/(taup*9.81)*(deltaPsi)+(derr);
	phi_d = fmax(fmin(phi_d,philim),-philim); // Saturate at +/- philim
	return phi_d;
	*/
}

// Takes a vector x in body coordinates and converts it to the inertial frame
void Controller::body2inertial(double psi, double th, double phi, double *x) {

	double y[3];

	// Local copy of x
	y[0] = x[0];
	y[1] = x[1];
	y[2] = x[2];

	double spsi = sin(psi); double cpsi = cos(psi);
	double sth  = sin(th);  double cth  = cos(th);
	double sphi = sin(phi); double cphi = cos(phi);

	double dcm[3][3];

	dcm[0][0] = cth*cpsi;
	dcm[1][0] = cth*spsi;
	dcm[2][0] = -sth;
	dcm[0][1] = sphi*sth*cpsi - cphi*spsi;
	dcm[1][1] = sphi*sth*spsi + cphi*cpsi;
	dcm[2][1] = sphi*cth;
	dcm[0][2] = cphi*sth*cpsi + sphi*spsi;
	dcm[1][2] = cphi*sth*spsi - sphi*cpsi;
	dcm[2][2] = cphi*cth;

	x[0] = dcm[0][0]*y[0] + dcm[0][1]*y[1] + dcm[0][2]*y[2];
	x[1] = dcm[1][0]*y[0] + dcm[1][1]*y[1] + dcm[1][2]*y[2];
	x[2] = dcm[2][0]*y[0] + dcm[2][1]*y[1] + dcm[2][2]*y[2];

	return;
}

// Takes a vector x in inertial coordinates and converts it to the body frame
void Controller::inertial2body(double psi, double th, double phi, double *x) {

	double y[3];

	// Local copy of x
	y[0] = x[0];
	y[1] = x[1];
	y[2] = x[2];

	double spsi = sin(psi); double cpsi = cos(psi);
	double sth  = sin(th);  double cth  = cos(th);
	double sphi = sin(phi); double cphi = cos(phi);

	double dcm[3][3];

	dcm[0][0] = cth*cpsi;
	dcm[1][0] = cth*spsi;
	dcm[2][0] = -sth;
	dcm[0][1] = sphi*sth*cpsi - cphi*spsi;
	dcm[1][1] = sphi*sth*spsi + cphi*cpsi;
	dcm[2][1] = sphi*cth;
	dcm[0][2] = cphi*sth*cpsi + sphi*spsi;
	dcm[1][2] = cphi*sth*spsi - sphi*cpsi;
	dcm[2][2] = cphi*cth;

	x[0] = dcm[0][0]*y[0] + dcm[1][0]*y[1] + dcm[2][0]*y[2];
	x[1] = dcm[0][1]*y[0] + dcm[1][1]*y[1] + dcm[2][1]*y[2];
	x[2] = dcm[0][2]*y[0] + dcm[1][2]*y[1] + dcm[2][2]*y[2];

	return;
}

// Computes c = a x b
void Controller::cross(double *a, double *b, double *c) {
	c[0] = a[1]*b[2]-a[2]*b[1];
	c[1] = a[2]*b[0]-a[0]*b[2];
	c[2] = a[0]*b[1]-a[1]*b[0];
	return;
}

void Controller::TECSlongitudinal() {
/*
	// Variables: tauh, tauV, Ktp, Kti, Kep, Kei

	double game, Vedot, h, hc, V, Vc, Vdot, hdot, Edotse, Edotde;

	// Get current airspeed, target airspeed, airspeed acceleration
	V    = computeAirspeed();
	Vc   = pv.Vtarget;
	Vdot = (V - VaL)/0.02;

	// Get current altitude, commanded altitude
	h  = -sv.Dp;
	hc = pv.waypointAlt[pv.wpidx];
	hdot = (h - hL)/0.02;

	// Compute flight path angle error (game ~= hdot_err/V)
	game = 1.0/V*((hc-h)/tauh - hdot);

	// Compute airspeed acceleration error
	Vedot = (Vc - V)/tauv - Vdot;

	// Compute specific energy error & derivative
	Ese  = (hc+0.5*9.81*Vc*Vc) - (h+0.5*9.81*V*V);
	Esed = game + Vedot/9.81;

	// Compute specific energy distribution error & derivative
	Ede  = (0.5*9.81*Vc*Vc-hc) - (0.5*9.81*V*V-h);
	Eded = Vedot/9.81-game;

	// Compute throttle input
	//thr_PWM = Ktp*Ese/V+Kti*Esed/V;

	// Compute elevator input

*/
}

void Controller::pbeta(double phides) {

	// Augmented state
	// phi_err, r_err, beta, p, r, phi
	double xclat[6], xdlat[6], dx[6], du[2], beta;

	double K[2][6]; // FORTRAN format

	K[0][0] = ctrlGns.cmap["Lat_K00"]; // Aileron gain on phi_err
	K[0][1] = ctrlGns.cmap["Lat_K01"]; // Aileron gain on r_err
	K[0][2] = ctrlGns.cmap["Lat_K02"]; // Aileron gain on beta
	K[0][3] = ctrlGns.cmap["Lat_K03"]; // Aileron gain on p
	K[0][4] = ctrlGns.cmap["Lat_K04"]; // Aileron gain on r
	K[0][5] = ctrlGns.cmap["Lat_K05"]; // Aileron gain on phi

	K[1][0] = ctrlGns.cmap["Lat_K10"]; // Rudder gain on phi_err
	K[1][1] = ctrlGns.cmap["Lat_K11"]; // Rudder gain on r_err
	K[1][2] = ctrlGns.cmap["Lat_K12"]; // Rudder gain on beta
	K[1][3] = ctrlGns.cmap["Lat_K13"]; // Rudder gain on p
	K[1][4] = ctrlGns.cmap["Lat_K14"]; // Rudder gain on r
	K[1][5] = ctrlGns.cmap["Lat_K15"]; // Rudder gain on phi

//	K[0][0] = 0.9990; // Aileron gain on phi_err
//	K[0][1] = 0.0044; // Aileron gain on r_err
//	K[0][2] = 0.0949; // Aileron gain on beta
//	K[0][3] = 0.0246; // Aileron gain on p
//	K[0][4] = 0.0080; // Aileron gain on r
//	K[0][5] = 0.5999; // Aileron gain on phi
//
//	K[1][0] = 0.0442; // Rudder gain on phi_err
//	K[1][1] = -0.0999; // Rudder gain on r_err
//	K[1][2] = -0.0095; // Rudder gain on beta
//	K[1][3] = -0.0003; // Rudder gain on p
//	K[1][4] = 0.0054; // Rudder gain on r
//	K[1][5] = 0.0098; // Rudder gain on phi

	beta = computeBeta();

	xclat = {phiErr, rErr, beta, sv.p, sv.r, sv.phi}; // Current state
	xdlat = {0,         0,    0,    0,    0, phides}; // Desired state

	for (int i = 0; i < 6; i++) {
		*(dx+i) = xclat[i]-xdlat[i];
	}

	// Compute -K*dx
	du[0] = -(K[0][0]*dx[0]+K[0][1]*dx[1]+K[0][2]*dx[2]+K[0][3]*dx[3]+
			K[0][4]*dx[4]+K[0][5]*dx[5]);
	du[1] = -(K[1][0]*dx[0]+K[1][1]*dx[1]+K[1][2]*dx[2]+K[1][3]*dx[3]+
			K[1][4]*dx[4]+K[1][5]*dx[5]);

	if (du[0] > philim) {
		du[0] = philim;
	} else if (du[0] < -philim) {
		du[0] = -philim;
	}

	ail_PWM = aileron->actuateRad(du[0]);
	cv.ailDefRad = fmax(fmin(du[0],ail_lim),-ail_lim);

	rud_PWM = rudder->actuateRad(du[1]);
	cv.rudDefRad = fmax(fmin(du[1],rud_lim),-rud_lim);

	return;
}

double Controller::computeBeta() {
	double Vb[3], Vbw[3], beta, Va_measured, Va_computed;
	Vb[0] = sv.VNi;  Vb[1]  = sv.VEi; Vb[2]  = sv.VDi;
	Vbw[0] = pv.VNi_wind; Vbw[1] = pv.VEi_wind; Vbw[2] = 0.0;
	inertial2body(sv.psi, sv.theta, sv.phi, Vb);
	inertial2body(sv.psi, sv.theta, sv.phi, Vbw);

	// Compute airspeed
	Va_measured = sqrt((apdat.diffpress-pv.diffb)/0.6125);
	Va_computed = sqrt((Vb[0]-Vbw[0])*(Vb[0]-Vbw[0])+
			           (Vb[1]-Vbw[1])*(Vb[1]-Vbw[1])+
		               (Vb[2]-Vbw[2])*(Vb[2]-Vbw[2]));

	// Do a comparison, if computed is > 3 m/s different, use measured
	if (fabs(Va_computed-Va_measured) > 3) {
		beta = asin((Vb[1]-Vbw[1])/Va_computed);
	} else {
		beta = asin((Vb[1]-Vbw[1])/Va_measured);
	}
	return beta;
}

double Controller::computeAirspeed() {
	double Vb[3], Vbw[3], Va_measured, Va_computed;
	Vb[0] = sv.VNi;  Vb[1]  = sv.VEi; Vb[2]  = sv.VDi;
	Vbw[0] = pv.VNi_wind; Vbw[1] = pv.VEi_wind; Vbw[2] = 0.0;
	inertial2body(sv.psi, sv.theta, sv.phi, Vb);
	inertial2body(sv.psi, sv.theta, sv.phi, Vbw);

	// Compute airspeed
	Va_measured = sqrt((apdat.diffpress-pv.diffb)/0.6125);
	Va_computed = sqrt((Vb[0]-Vbw[0])*(Vb[0]-Vbw[0])+
			           (Vb[1]-Vbw[1])*(Vb[1]-Vbw[1])+
		               (Vb[2]-Vbw[2])*(Vb[2]-Vbw[2]));

	// Do a comparison, if computed is > 3 m/s different, return measured
	if (fabs(Va_computed-Va_measured) > 3) {
		return Va_measured;
	} else {
		return Va_computed;
	}
}

void Controller::lateralPID(double phides) {

/*	double phi_err, dail, drud, Kphi, Kdp, Kr;

	Kphi = ctrlGns.gains[0];
	Kdp  = ctrlGns.gains[1];
	Kr   = ctrlGns.gains[2];

	// Compute the error in the roll angle
	phi_err = phides - sv.phi;

	// Set is the aileron command
	dail = Kphi*phi_err + Kdp*sv.p;
	ail_PWM = aileron->actuateRad(dail);
	cv.ailDefRad = fmax(fmin(dail,ail_lim),-ail_lim);

	// 're' is the signal after r is fed through the washout filter
	re[1] = 0.9976*re[0] + sv.r - rL;
	// 'drc' is the rudder command (want 0 yaw rate, i.e. r = 0 so drc = (0 - Kr*re)
	drud = -Kr*re[1];
	rud_PWM = rudder->actuateRad(drud);
	cv.rudDefRad = fmax(fmin(drud,rud_lim),-rud_lim);

	return;
	*/
}

void Controller::airspeedPID() {
	/*
	// Declare local variables
	double KpVa, KdVa, KpVaphi, Va, Va_err[2], Va_err_dot, dele;

	// Unpack gains
	KpVa = ctrlGns.gains[3];
	KdVa = ctrlGns.gains[4];
	KpVaphi = ctrlGns.gains[5];

	// Airspeed PID loop
	Va = computeAirspeed();
	Va_err[0] = Vtarget - Va;
	Va_err[1] = Vtarget - VaL;
	Va_err_dot = (Va_err[0]-Va_err[1])/(0.02);
	Va_err_i[1] = Va_err_i[0] + Va_err[0]*(0.02);

	if (Va_err_i[1] > 5) {
		Va_err_i[1] = 5.0;
	} else if (Va_err_i[1] < -5) {
		Va_err_i[1] = -5.0;
	}

	dele = KpVa*Va_err[0] + KdVa*Va_err_dot + KpVaphi*sv.phi;
	//+KdVa*Va_err_dot+KiVa*Va_err_i[1]+K1*fabs(sv.phi)+k2*SV.P + ele_trim;

	ele_PWM = elevator->actuateRad(dele);
	cv.eleDefRad = fmax(fmin(dele,ele_lim),-ele_lim);

	VaL = Va;
	*/
}

void Controller::updateControlFile() {
	fw->updateValue("ailDefRad",cv.ailDefRad);
	fw->updateValue("eleDefRad",cv.eleDefRad);
	fw->updateValue("rudDefRad",cv.rudDefRad);
	fw->updateValue("thrPercent",cv.thrPercent);
}

void Controller::LQRPI_V_alt() {
	// Augmented state
	// phi_err, r_err, beta, p, r, phi
	double xclon[6], xdlon[6], dx[6], du[2], V;

	double K[2][6]; // FORTRAN format

	K[0][0] = ctrlGns.cmap["Lon_K00"]; // Elevator gain on dErr
	K[0][1] = ctrlGns.cmap["Lon_K01"];;  // Elevator gain on vErr
	K[0][2] = ctrlGns.cmap["Lon_K02"];;  // Elevator gain on V
	K[0][3] = ctrlGns.cmap["Lon_K03"];; // Elevator gain on q
	K[0][4] = ctrlGns.cmap["Lon_K04"];; // Elevator gain on theta
	K[0][5] = ctrlGns.cmap["Lon_K05"];;  // Elevator gain on D

	K[1][0] = ctrlGns.cmap["Lon_K10"]; // Throttle gain on dErr
	K[1][1] = ctrlGns.cmap["Lon_K11"]; // Throttle gain on vErr
	K[1][2] = ctrlGns.cmap["Lon_K12"]; // Throttle gain on V
	K[1][3] = ctrlGns.cmap["Lon_K13"]; // Throttle gain on q
	K[1][4] = ctrlGns.cmap["Lon_K14"]; // Throttle gain on theta
	K[1][5] = ctrlGns.cmap["Lon_K15"]; // Throttle gain on D

//	K[0][0] = 0.0638;  // Elevator gain on dErr
//	K[0][1] = 0.0861;  // Elevator gain on vErr
//	K[0][2] = 0.0709;  // Elevator gain on V
//	K[0][3] = -0.4284; // Elevator gain on q
//	K[0][4] = -1.4167; // Elevator gain on theta
//	K[0][5] = 0.1477;  // Elevator gain on D
//
//	K[1][0] = -0.0304; // Throttle gain on dErr
//	K[1][1] = 0.1805; // Throttle gain on vErr
//	K[1][2] = 0.1848; // Throttle gain on V
//	K[1][3] = -0.0073; // Throttle gain on q
//	K[1][4] = -0.2784; // Throttle gain on theta
//	K[1][5] = -0.0835; // Throttle gain on D

//	K[0][0] = 0.0390;  // Elevator gain on dErr
//	K[0][1] = 0.0981;  // Elevator gain on vErr
//	K[0][2] = 0.0857;  // Elevator gain on V
//	K[0][3] = -0.4253; // Elevator gain on q
//	K[0][4] = -1.2805; // Elevator gain on theta
//	K[0][5] = 0.0992;  // Elevator gain on D
//
//	K[1][0] = -0.0219; // Throttle gain on dErr
//	K[1][1] = 0.1743; // Throttle gain on vErr
//	K[1][2] = 0.1761; // Throttle gain on V
//	K[1][3] = -0.0086; // Throttle gain on q
//	K[1][4] = -0.3350; // Throttle gain on theta
//	K[1][5] = -0.0641; // Throttle gain on D

	V = computeAirspeed();

	xclon = {dErr, vErr, V,       sv.q, sv.theta, sv.Dp}; // Current state
	xdlon = {0,    0,    Vtarget, 0,    0.05,     dDes}; // Desired state

	for (int i = 0; i < 6; i++) {
		*(dx+i) = xclon[i]-xdlon[i];
	}

	// Saturate difference in altitude between current and desired
	dx[5] = fmax(fmin(dx[5],3),-3);

	// Compute -K*dx
	du[0] = -(K[0][0]*dx[0]+K[0][1]*dx[1]+K[0][2]*dx[2]+K[0][3]*dx[3]+
			K[0][4]*dx[4]+K[0][5]*dx[5]) - 0.0192;
	du[1] = -(K[1][0]*dx[0]+K[1][1]*dx[1]+K[1][2]*dx[2]+K[1][3]*dx[3]+
			K[1][4]*dx[4]+K[1][5]*dx[5]) + 0.4721;

	du[0] = fmax(fmin(du[0],ele_lim),-ele_lim);
	du[1] = fmax(fmin(du[1],1),0);

	ele_PWM = elevator->actuateRad(du[0]);
	cv.eleDefRad = du[0];

	// Scale 0 to 200
	thr_PWM = 85 + 115*du[1];
	cv.thrPercent = du[1];

	return;
}

void Controller::sysIDLong() {

	double instPhi, y, t1, f1, f0, t, amp;

	t1 = 5.0;
	f0 = 0.5;
	f1 = 5.0;
	t  = 0.02*sysctrLon;
	amp = 8*M_PI/180.0;

    instPhi = t1/log(f1/f0)*(f0*pow((f1/f0),(t/t1))-f0);
    y = amp*cos(2*M_PI*instPhi+90.0/360.0) - 0.05;
    ele_PWM = elevator->actuateRad(y);
    cv.eleDefRad = y;
    sysctrLon++;
    if (sysctrLon > 250) {
    	sysctrLon = 0;
    }
}

void Controller::sysIDLat() {

	double instPhi, y, t1, f1, f0, t, amp;

	t1 = 5.0;
	f0 = 0.5;
	f1 = 5.0;
	t  = 0.02*sysctrLat;
	amp = 5*M_PI/180.0;

    instPhi = t1/log(f1/f0)*(f0*pow((f1/f0),(t/t1))-f0);
    y = amp*cos(2*M_PI*instPhi+90.0/360.0) - 0.05;
    ail_PWM = aileron->actuateRad(-y);
    rud_PWM = rudder->actuateRad(2*y);
    cv.ailDefRad = -y;
    cv.rudDefRad = 2*y;
    sysctrLat++;
    if (sysctrLat > 250) {
    	sysctrLat = 0;
    }
}
